/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package xenon3d.vector;

/**
 * A four-element axis angle represented by single-precision floating point
 * x,y,z,angle components.  An axis-angle is a rotation of angle (degrees)
 * about the vector (x,y,z). Note that the axis-angle is never automatically
 * normalized. Use the normalize() method if normalization is required.
 * @author Volker Everts
 * @version 0.1 - 18.09.2011: Created
 */
public class AxisAngle4f extends Tuple4f implements Normalizable {

    // <editor-fold defaultstate="collapsed" desc=" Static Attributes ">

    /** The serial version UID. */
    private static final long serialVersionUID = 20110917L;

    // </editor-fold>

    // <editor-fold defaultstate="collapsed" desc=" Initialization ">

    /**
     * Constructs and initializes a normalized AxisAngle4f using an axis vector
     * (0,1,0) and a rotation of 0 degrees.
     */
    public AxisAngle4f() {
        super(0.0f, 1.0f, 0.0f, 0.0f);
    }

    /**
     * Constructs and initializes an AxisAngle4f from the specified x, y and z
     * coordinates and an angle in degrees.
     * @param x the x coordinate
     * @param y the y coordinate
     * @param z the z coordinate
     * @param w the angle of rotation in degrees
     */
    public AxisAngle4f(float x, float y, float z, float w) {
        super(x, y, z, w);
    }

    /**
     * Constructs and initializes an AxisAngle4f from the specified axis and
     * angle.
     * @param axis the axis vector
     * @param w the angle of rotation in degrees
     */
    public AxisAngle4f(Vector3f axis, float w) {
        super(axis.x, axis.y, axis.z, w);
    }

    /**
     * Constructs and initializes an AxisAngle4f from the specified other
     * axis angle.
     * @param a the other axis angle
     */
    public AxisAngle4f(AxisAngle4f a) {
        super(a);
    }

    // </editor-fold>

    // <editor-fold defaultstate="collapsed" desc=" Public Properties (General) ">

    /**
     * Sets the value of this axis-angle to the specified axis vector and angle.
     * @param axis the axis vector
     * @param w the angle of rotation in degrees
     */
    public final void set(Vector3f axis, float w) {
        x = axis.x;
        y = axis.y;
        z = axis.z;
        this.w = w;
    }

    /**
     * Gets the axis vector and the angle of rotation for this axis-angle.
     * @param result the axis vector of this axis-angle
     * @return the angle of rotation in degrees
     */
    public final float get(Vector3f result) {
        result.set(x, y, z);
        return w;
    }

    // </editor-fold>

    // <editor-fold defaultstate="collapsed" desc=" Public Properties (Conversions) ">

    /**
     * Sets the value of this axis-angle to the normalized rotational equivalent
     * of the specified euler-angles.<p>
     * Equations<p>
     * <code>
     * x = s1 s2 c3 +c1 c2 s3
     * y = s1 c2 c3 + c1 s2 s3
     * z = c1 s2 c3 - s1 c2 s3
     * w = 2 * acos(c1c2c3 - s1s2s3)
     * </code>
     * @param e the EulerAngle3f
     */
    public final void set(EulerAngle3f e) {
        float c1 = FMath.cos(e.y * 0.5f);
        float s1 = FMath.sin(e.y * 0.5f);
        float c2 = FMath.cos(e.x * 0.5f);
        float s2 = FMath.sin(e.x * 0.5f);
        float c3 = FMath.cos(e.z * 0.5f);
        float s3 = FMath.sin(e.z * 0.5f);
        x = c1 * c2 * s3 + s1 * s2 * c3;
        y = s1 * c2 * c3 + c1 * s2 * s3;
        z = c1 * s2 * c3 - s1 * c2 * s3;
        w = c1 * c2 * c3 - s1 * s2 * s3;
        w = 2.0f * FMath.acos(w);
        normalize();
    }

    /**
     * Sets the value of this axis-angle to the normalized rotational equivalent
     * of the passed quaternion. If the specified quaternion has no rotational
     * component, the value of this AxisAngle4f is set to an angle of 0 about
     * an axis of (0,1,0).<p>
     * Equations<p>
     * <code>
     * x = qx / sqrt(1-qw*qw)
     * y = qy / sqrt(1-qw*qw)
     * z = qz / sqrt(1-qw*qw)
     * w = 2 * acos(qw)
     * </code>
     * @param q the Quat4f
     */
    public final void set(Quat4f q) {
        float mag = q.normSq();
        if (mag > FMath.EPS) {
            if (FMath.isEqual(mag, 1.0f, FMath.EPS)) {
                x = q.x;
                y = q.y;
                z = q.z;
            }
            else {
                mag = FMath.sqrt(mag);
                x = q.x / mag;
                y = q.y / mag;
                z = q.z / mag;
            }
            w = 2.0f * FMath.atan2(mag, q.w);
        }
        else set(0.0f, 1.0f, 0.0f, 0.0f);
    }

    /**
     * Sets the value of this axis-angle to the rotational component of the
     * passed rotation matrix. If the specified matrix has no rotational
     * component, the value of this AxisAngle4f is set to an angle of 0 about
     * an axis of (0,1,0).
     * @param mat the Matrix3f
     */
    public final void set(Matrix3f mat) {
        x = mat.m32 - mat.m23;
        y = mat.m13 - mat.m31;
        z = mat.m21 - mat.m12;
        float mag = x * x + y * y + z * z;
        if (mag > FMath.EPS) {
            mag = FMath.sqrt(mag);
            float sn = 0.5f * mag;
            float cs = 0.5f * (mat.m11 + mat.m22 + mat.m33 - 1.0f);
            w = FMath.atan2(sn, cs);
            x /= mag;
            y /= mag;
            z /= mag;
        }
        else set(0.0f, 1.0f, 0.0f, 0.0f);
    }

    /**
     * Sets the value of this axis-angle to the rotational component of the
     * passed transformation matrix. If the specified matrix has no rotational
     * component, the value of this AxisAngle4f is set to an angle of 0 about
     * an axis of (0,1,0).
     * @param mat the Matrix4f
     */
    public final void set(Matrix4f mat) {
        x = mat.m32 - mat.m23;
        y = mat.m13 - mat.m31;
        z = mat.m21 - mat.m12;
        float mag = x * x + y * y + z * z;
        if (mag > FMath.EPS) {
            mag = FMath.sqrt(mag);
            float sn = 0.5f * mag;
            float cs = 0.5f * (mat.m11 + mat.m22 + mat.m33 - 1.0f);
            w = FMath.atan2(sn, cs);
            x /= mag;
            y /= mag;
            z /= mag;
        }
        else set(0.0f, 1.0f, 0.0f, 0.0f);
    }

    // </editor-fold>

    // <editor-fold defaultstate="collapsed" desc=" Public Methods ">

    /**
     * Adds the specified value to the angle of rotation and modulates the angle
     * so that it is between -180 and +180 degrees.
     * @param delta the angle to add
     */
    public final void addAngle(float delta) {
        w += delta;
        if (w > 180.0f) w -= 360.0f;
        else if (w < -180.0f) w += 360.0f;
    }

    // </editor-fold>

    // <editor-fold defaultstate="collapsed" desc=" Implementation Normalizable ">

    /**
     * Normalizes this axis-angle in place. During normalization the axis angle
     * will be normalized to a unit vector.
     */
    public final void normalize() {
        float mag = normSq();
        if (FMath.isEqual(mag, 1.0f, FMath.EPS)) return;
        if (mag > FMath.EPS) {
            mag = FMath.sqrt(mag);
            this.x /= mag;
            this.y /= mag;
            this.z /= mag;
        }
        else set(0.0f, 1.0f, 0.0f, 0.0f);
    }

    /**
     * Returns the norm of this axis-angle (the length of the axis vector).
     * @return the length of the axis vector
     */
    public final float norm() {
        return FMath.sqrt(normSq());
    }

    /**
     * Returns the squared norm of this axis-angle (the squared length of the
     * axis vector).
     * @return the squared length of the axis vector
     */
    public final float normSq() {
        return x * x + y * y + z * z;
    }

    // </editor-fold>

} // end class AxisAngle4f